#include "hal.h"

//Procedure di inizializzazione

volatile unsigned long C1_p,C2_p;
volatile enum {c_first,c_OK} C1_state,C2_state;
volatile unsigned char C1_HB,C2_HB;
volatile unsigned int Cap1_old,Cap2_old;



void HAL_init(void)
{         
    // PORTS
    
    P1OUT=0x00;
    P1DIR=0x01;    
    P1SEL=BIT3+BIT2;
    
/*
    P2OUT=
    P2DIR=
*/


    P3OUT=0x00;
    P3DIR=BIT3;

/*
    P4OUT=
    P4DIR=

    P5OUT=
    P5DIR=
    */  

    P6OUT= 0x00;
    P6DIR= 0x3F;
    
    // System Clock
    // BCSCTL2,BCSCTL1,DCOCTL
    DCOCTL=DCO0+DCO1+DCO2;  // DCO=7  
    BCSCTL1=RSEL0+RSEL1+RSEL2+XT2OFF; // RSEL=7
    BCSCTL2=0;  //
    
    WDTCTL = WDTPW | WDTHOLD;                      // stop watchdog timer
    
    BCSCTL1 |= XTS;                                // XT1 as high-frequency
    _BIC_SR(OSCOFF);                               // turn on XT1 oscillator
                          
    do                                             // wait in loop until crystal is stable 
      IFG1 &= ~OFIFG;
    while (IFG1 & OFIFG);
                                                           
    IFG1 &= ~OFIFG;                                // clear osc. fault int. flag
    BCSCTL2 = SELM0 | SELM1;
    
    // TIMERA
    //
    TACTL=TASSEL_1+TACLR+TAIE;   // SMCLK
    TACTL |= MC_2;       // Continuos mode, go  
    // Modulo 0
    TACCTL0=CCIE;
    // Modulo 1
    TACCTL1=CAP;
    TACCTL1|= CCIS_0+CCIE+SCS+CM_1;   // P1.2
    // Modulo 2
    TACCTL2=CAP;
    TACCTL2|= CCIS_0+CCIE+SCS+CM_1;    // P1.3
    
    C1_state=C2_state=c_first;
    C1_p=C2_p=0xFFFFFFFFl;
    
    // Flash module
    FCTL2 = FWKEY + FSSEL_2 + 20;   // SMCLK/20 for Flash Timing Generator
}    

// HW TICK TIMER MODULE

void (*TickCB)(void);
volatile unsigned int Period;

unsigned char HAL_AllocSyncTickEvent( void (*CB)(void),unsigned int micros,unsigned int Fase){
    TickCB=CB;
    Period=micros*4;
    CCR0=Period;
    _EINT();
    return(1);
}
#pragma vector=TIMERA0_VECTOR
__interrupt  void TMRA_CC0_Handler(void) {       
      CCR0+=Period;      
      (*TickCB)();
}

// HW CAPTURE PERIOD MODULE

unsigned long HAL_GetPeriodC1(void){
      return(C1_p);
}

unsigned long HAL_GetPeriodC2(void){
      return(C2_p);
}

void HAL_StartC2(void){

}
void HAL_StartC1(void){

}
void HAL_StopC1(void){

}
void HAL_StopC2(void){

}

#pragma vector=TIMERA1_VECTOR
__interrupt void TMRA_MUX_Handler(void)
{
     unsigned int buffer;
     
 switch( TAIV )
 {
   
   case  2: /* CCR1 */
            switch( C1_state )
            {
              case  c_first:
                C1_p=0xFFFFFFFFl;
                Cap1_old=CCR1;
                C1_state=c_OK;
                C1_HB=0;
                break;
              case  c_OK:
                buffer = CCR1;              
                if(buffer>Cap1_old){
                  C1_p=(buffer-Cap1_old)+65536l*(C1_HB);
                  C1_HB=0;                          
                }
                else {
                  C1_p=65536l*(C1_HB)-(Cap1_old-buffer);
                  C1_HB=0;                                
                }                              
                Cap1_old = buffer;
                if(TACCTL1 & COV) {
                  C1_state=c_first;
                  C1_p=0;
                  TACCTL1 &= ~COV;        
                }
                break;
            }      
            break;
            
   case  4: /* CCR2 */ 
            switch( C2_state )
            {

              case  c_first:
                C2_p=0xFFFFFFFFl;
                Cap2_old=CCR2;
                C2_state=c_OK;
                C2_HB=0;
                break;
              
              case  c_OK:
                buffer = CCR2;
                if(buffer>Cap2_old){
                  C2_p=(buffer-Cap2_old)+65536l*(C2_HB);
                  C2_HB=0;                          
                }
                else {
                  C2_p=65536l*(C2_HB)-(Cap2_old-buffer);
                  C2_HB=0;                                
                }                              
                Cap2_old = buffer;
                if(TACCTL2 & COV) {
                  C2_state=c_first;
                  C2_p=0l;
                  TACCTL2 &= ~COV;        
                }
                break;
            }                 
            break;              
   case  6: break;              /* ---- */
   case  8: break;              /* ---- */
   case 10: /* Timer_A0 overflow        */
            TACTL&= ~TAIFG;                         
            if(C1_HB!=0xFF)C1_HB++;else {
                  C1_state=c_first;
                  C1_p=0xFFFFFFFFl;
                }
            if(C2_HB!=0xFF)C2_HB++;else {
                  C2_state=c_first;
                  C2_p=0xFFFFFFFFl;        
                }             
            break;              
   case 12: break;              /* ---- */
   case 14: break;              /* ---- */            
 }
}

// FLASH MODULE HANDLE

void HAL_copy_A2B (void){

  char *Flash_ptrA;               // Segment A pointer
  char *Flash_ptrB;               // Segment B pointer
  unsigned int i;
  
  _DINT();  
  Flash_ptrA = (char *) 0x1000;   // Initialize Flash segment A pointer
  Flash_ptrB = (char *) 0x1080;   // Initialize Flash segment B pointer  
  FCTL1 = FWKEY + ERASE;          // Set Erase bit
  FCTL3 = FWKEY;                  // Clear Lock bit
  *Flash_ptrB = 0;                //Dummy write to erase Flash segment B
  FCTL1 = FWKEY + WRT;            // Set WRT bit for write operation 

  for (i=0; i<128; i++)
  {
    *Flash_ptrB++ = *Flash_ptrA++; // copy value segment A to segment B
  }
  
  FCTL1 = FWKEY;                  // Clear WRT bit
  FCTL3 = FWKEY + LOCK;           // Reset LOCK bit
  _EINT();
  
}

void *HAL_NVprom2ram(void *ramdest,void *eepromsrc,size_t size){
     
     
     while(size){
         //*((unsigned char *)ramdest)++ = *((unsigned char *)eepromsrc)++;
         size--;
     }
     return(ramdest);
}

void *HAL_NV_ram2shot(void *Source,size_t size){
  char *Flash_ptr;                // Flash pointer
  
  _DINT();
  Flash_ptr = (char *) 0x1000;    // Initialize Flash pointer  
  FCTL1 = FWKEY + ERASE;          // Set Erase bit
  FCTL3 = FWKEY;                  // Clear Lock bit
  *Flash_ptr = 0;                 //Dummy write to erase Flash segment
  
  FCTL1 = FWKEY + WRT;            // Set WRT bit for write operation

  while(size ){
         //*Flash_ptr++ = *((unsigned char *)Source)++;
         size--;
  }
    
  FCTL1 = FWKEY;                  // Clear WRT bit
  FCTL3 = FWKEY + LOCK;           // Reset LOCK bit
  _EINT();
  return(Source);
}

void *HAL_NVram2prom(void *eepromdest,void *ramsrc,size_t size){
      char *Flash_ptr,*Flash_ptrB;
      HAL_copy_A2B();            
   
      _DINT();
      Flash_ptr = (char *) 0x1000;    // Initialize Flash pointer  
      Flash_ptrB = (char *) 0x1080;
      FCTL1 = FWKEY + ERASE;          // Set Erase bit
      FCTL3 = FWKEY;                  // Clear Lock bit
      *Flash_ptr = 0;                 //Dummy write to erase Flash segment
  
      FCTL1 = FWKEY + WRT;            // Set WRT bit for write operation
      
      while(Flash_ptr<eepromdest){
         *Flash_ptr++ = *Flash_ptrB++;
      }     
      while(size ){
         //*Flash_ptr++ = *((unsigned char *)ramsrc)++;
         size--;
         Flash_ptrB++;
      }
    
      while(Flash_ptr < (char *) 0x1080){
         *Flash_ptr++ = *Flash_ptrB++;
      } 
    
    
      FCTL1 = FWKEY;                  // Clear WRT bit
      FCTL3 = FWKEY + LOCK;           // Reset LOCK bit
      _EINT();
      return(Flash_ptr);
}